/**
 * @file ebex_matrix.c
 *
 * @date Dec 4, 2009
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2009 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <math.h>
#include <float.h>
#include <string.h>

#include <ebex_vector.h>
#include <ebex_quaternion.h>
#include <ebex_matrix.h>
#include <ebex_math.h>

/*
Tridiagonal matrix from symmetric 4x4 matrix using Housholder reduction.
The input matrix is altered.
*/
int tridiagonalize_symmetric_44(
    double *matrix,      /* double[16] */
    double *diagonal,    /* double[4] */
    double *subdiagonal) /* double[3] */
{
    double t, n, g, h;
    double v0, v1, v2;
    double *M = matrix;
    double *u;

    u = &M[1];
    t = u[1]*u[1] + u[2]*u[2];
    n = sqrt(u[0]*u[0] + t);
    if (n > DBL_EPSILON) {
        if (u[0] < 0.0)
            n = -n;
        u[0] += n;
        h = (u[0]*u[0] + t) / 2.0;
        v0 = M[5]*u[0] + M[6]*u[1]  + M[7]*u[2];
        v1 = M[6]*u[0] + M[10]*u[1] + M[11]*u[2];
        v2 = M[7]*u[0] + M[11]*u[1] + M[15]*u[2];
        v0 /= h;
        v1 /= h;
        v2 /= h;
        g = (u[0]*v0 + u[1]*v1 + u[2]*v2) / (2.0 * h);
        v0 -= g * u[0];
        v1 -= g * u[1];
        v2 -= g * u[2];
        M[5] -=  2.0*v0*u[0];
        M[10] -= 2.0*v1*u[1];
        M[15] -= 2.0*v2*u[2];
        M[6]  -= v1*u[0] + v0*u[1];
        M[7]  -= v2*u[0] + v0*u[2];
        M[11] -= v2*u[1] + v1*u[2];
        M[1] = -n;
    }

    u = &M[6];
    t = u[1]*u[1];
    n = sqrt(u[0]*u[0] + t);
    if (n > DBL_EPSILON) {
        if (u[0] < 0.0)
            n = -n;
        u[0] += n;
        h = (u[0]*u[0] + t) / 2.0;
        v0 = M[10]*u[0] + M[11]*u[1];
        v1 = M[11]*u[0] + M[15]*u[1];
        v0 /= h;
        v1 /= h;
        g = (u[0]*v0 + u[1]*v1) / (2.0 * h);
        v0 -= g * u[0];
        v1 -= g * u[1];
        M[10] -= 2.0*v0*u[0];
        M[15] -= 2.0*v1*u[1];
        M[11] -= v1*u[0] + v0*u[1];
        M[6] = -n;
    }

    diagonal[0] = M[0];
    diagonal[1] = M[5];
    diagonal[2] = M[10];
    diagonal[3] = M[15];
    subdiagonal[0] = M[1];
    subdiagonal[1] = M[6];
    subdiagonal[2] = M[11];

    return 0;
}

/*
Return largest eigenvalue of symmetric tridiagonal matrix.
Matrix Algorithms: Basic decompositions. By GW Stewart. Chapter 3.
*/
double max_eigenvalue_of_tridiag_44(
    double *diagonal,    /* double[4] */
    double *subdiagonal) /* double[3] */
{
    int count;
    double lower, upper, t0, t1, d, eps, eigenv;
    double *a = diagonal;
    double *b = subdiagonal;

    /* upper and lower bounds using Gerschgorin's theorem */
    t0 = fabs(b[0]);
    t1 = fabs(b[1]);
    lower = a[0] - t0;
    upper = a[0] + t0;
    d = a[1] - t0 - t1;
    lower = MIN(lower, d);
    d = a[1] + t0 + t1;
    upper = MAX(upper, d);
    t0 = fabs(b[2]);
    d = a[2] - t0 - t1;
    lower = MIN(lower, d);
    d = a[2] + t0 + t1;
    upper = MAX(upper, d);
    d = a[3] - t0;
    lower = MIN(lower, d);
    d = a[3] + t0;
    upper = MAX(upper, d);

    /* precision */
    eps = (4.0 * (fabs(lower) + fabs(upper))) * DBL_EPSILON;

    /* interval bisection until width is less than tolerance */
    while (fabs(upper - lower) > eps) {

        eigenv = (upper + lower) / 2.0;
        if ((eigenv == upper) || (eigenv == lower))
            return eigenv;

        /* counting pivots < 0 */
        d = a[0] - eigenv;
        count = (d < 0.0) ? 1 : 0;
        if (fabs(d) < eps)
            d = eps;
        d = a[1] - eigenv - b[0]*b[0] / d;
        if (d < 0.0)
            count++;
        if (fabs(d) < eps)
            d = eps;
        d = a[2] - eigenv - b[1]*b[1] / d;
        if (d < 0.0)
            count++;
        if (fabs(d) < eps)
            d = eps;
        d = a[3] - eigenv - b[2]*b[2] / d;
        if (d < 0.0)
            count++;

        if (count < 4)
            lower = eigenv;
        else
            upper = eigenv;
    }

    return (upper + lower) / 2.0;
}

/*
Eigenvector of symmetric tridiagonal 4x4 matrix using Cramer's rule.
*/
int eigenvector_of_symmetric_44(
    double *matrix, /* double[16] */
    double *vector, /* double[4]  */
    double *buffer) /* double[12] */
{
    double n, eps;
    double *M = matrix;
    double *v = vector;
    double *t = buffer;

    /* eps: minimum length of eigenvector to use */
    eps = (M[0]*M[5]*M[10]*M[15] - M[1]*M[1]*M[11]*M[11]) * 1e-6;
    eps *= eps;
    if (eps < DBL_EPSILON)
        eps = DBL_EPSILON;

    t[0] = M[10] * M[15];
    t[1] = M[11] * M[11];
    t[2] = M[6] *  M[15];
    t[3] = M[11] * M[7];
    t[4] = M[6] *  M[11];
    t[5] = M[10] * M[7];
    t[6] = M[2] *  M[15];
    t[7] = M[11] * M[3];
    t[8] = M[2] *  M[11];
    t[9] = M[10] * M[3];
    t[10] = M[2] * M[7];
    t[11] = M[6] * M[3];

    v[0] =  t[1]*M[1] + t[6]*M[6] + t[9]*M[7];
    v[0] -= t[0]*M[1] + t[7]*M[6] + t[8]*M[7];
    v[1] =  t[2]*M[1] + t[7]*M[5] + t[10]*M[7];
    v[1] -= t[3]*M[1] + t[6]*M[5] + t[11]*M[7];
    v[2] =  t[5]*M[1] + t[8]*M[5] + t[11]*M[6];
    v[2] -= t[4]*M[1] + t[9]*M[5] + t[10]*M[6];
    v[3] =  t[0]*M[5] + t[3]*M[6] + t[4]*M[7];
    v[3] -= t[1]*M[5] + t[2]*M[6] + t[5]*M[7];
    n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];

    if (n < eps) {
        v[0] =  t[0]*M[0] + t[7]*M[2] + t[8]*M[3];
        v[0] -= t[1]*M[0] + t[6]*M[2] + t[9]*M[3];
        v[1] =  t[3]*M[0] + t[6]*M[1] + t[11]*M[3];
        v[1] -= t[2]*M[0] + t[7]*M[1] + t[10]*M[3];
        v[2] =  t[4]*M[0] + t[9]*M[1] + t[10]*M[2];
        v[2] -= t[5]*M[0] + t[8]*M[1] + t[11]*M[2];
        v[3] =  t[1]*M[1] + t[2]*M[2] + t[5]*M[3];
        v[3] -= t[0]*M[1] + t[3]*M[2] + t[4]*M[3];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps) {
        t[0]  = M[2] * M[7];
        t[1]  = M[3] * M[6];
        t[2]  = M[1] * M[7];
        t[3]  = M[3] * M[5];
        t[4]  = M[1] * M[6];
        t[5]  = M[2] * M[5];
        t[6]  = M[0] * M[7];
        t[7]  = M[3] * M[1];
        t[8]  = M[0] * M[6];
        t[9]  = M[2] * M[1];
        t[10] = M[0] * M[5];
        t[11] = M[1] * M[1];

        v[0] =  t[1]*M[3] + t[6]*M[11] + t[9]*M[15];
        v[0] -= t[0]*M[3] + t[7]*M[11] + t[8]*M[15];
        v[1] =  t[2]*M[3] + t[7]*M[7] + t[10]*M[15];
        v[1] -= t[3]*M[3] + t[6]*M[7] + t[11]*M[15];
        v[2] =  t[5]*M[3] + t[8]*M[7] + t[11]*M[11];
        v[2] -= t[4]*M[3] + t[9]*M[7] + t[10]*M[11];
        v[3] =  t[0]*M[7] + t[3]*M[11] + t[4]*M[15];
        v[3] -= t[1]*M[7] + t[2]*M[11] + t[5]*M[15];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps) {
        v[0] =  t[8]*M[11] + t[0]*M[2] + t[7]*M[10];
        v[0] -= t[6]*M[10] + t[9]*M[11] + t[1]*M[2];
        v[1] =  t[6]*M[6] + t[11]*M[11] + t[3]*M[2];
        v[1] -= t[10]*M[11] + t[2]*M[2] + t[7]*M[6];
        v[2] =  t[10]*M[10] + t[4]*M[2] + t[9]*M[6];
        v[2] -= t[8]*M[6] + t[11]*M[10] + t[5]*M[2];
        v[3] =  t[2]*M[10] + t[5]*M[11] + t[1]*M[6];
        v[3] -= t[4]*M[11] + t[0]*M[6] + t[3]*M[10];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps)
        return -1;

    n = sqrt(n);
    v[0] /= n;
    v[1] /= n;
    v[2] /= n;
    v[3] /= n;

    return 0;
}

void ebex_mat_eigenv_4x4(ebex_mat4x4_t *m_eigenvectors, ebex_quat_t *m_output, ebex_mat4x4_t *m_input)
{
    double l;
    double diag[4];
    double subdiag[3];
    double buffer[12];

    ebex_mat4x4_t tmp_mat;
    ebex_quat_t retval;

    memcpy(&tmp_mat, m_input, sizeof(ebex_mat4x4_t));

    if (tridiagonalize_symmetric_44(tmp_mat.element, diag, subdiag) != 0) {
        die("tridiagonalize_symmetric_44() failed");

    }

    l = max_eigenvalue_of_tridiag_44(diag, subdiag);
    m_eigenvectors->element[0] -= l;
    m_eigenvectors->element[5] -= l;
    m_eigenvectors->element[10] -= l;
    m_eigenvectors->element[15] -= l;

    if (eigenvector_of_symmetric_44(m_eigenvectors->element, retval.element, buffer) != 0) {
        die("eigenvector_of_symmetric_44() failed");
    }

    m_output->element[0] = retval.element[0];
    m_output->element[3] = retval.element[3];
    m_output->element[2] = retval.element[2];
    m_output->element[1] = retval.element[1];

}

// Symmetric Householder reduction to tridiagonal form.
void ebex_mat_tridiag_reduce_4x4(ebex_mat4x4_t *m_matrix, ebex_mat4x4_t *m_diagonal, ebex_mat4x4_t *m_offdiagonal)
{
	//  This is derived from the Algol procedures tred2 by
	//  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
	//  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
	//  Fortran subroutine in EISPACK.

	for (int j = 0; j < 4; ++j)
	{
		m_diagonal->col[j].element[j] = m_matrix->col[3].element[j];
	}

	// Householder reduction to tridiagonal form.
	for (int i = 3; i > 0; i--)
	{
		// Scale to avoid under/overflow.

		double scale = 0.0;
		double h = 0.0;

		for (int k = 1; k < i; ++k)
		{
			scale = scale + fabs(m_diagonal->col[k].element[k]);
		}

		if (scale < DBL_EPSILON)
		{
			m_offdiagonal->col[i].element[i] = m_diagonal->col[i - 1].element[i - 1];
			for (int j = 1; j < i; j++)
			{
				m_diagonal->col[j].element[j] = m_matrix->col[i - 1].element[j];
				m_matrix->col[i].element[j] = 0;
				m_matrix->col[j].element[i] = 0;
			}
		}
		else
		{
			// Generate Householder vector.

			for (int k = 0; k < i; ++k)
			{
				m_diagonal->col[k].element[k] /= scale;
				h += m_diagonal->col[k].element[k] * m_diagonal->col[k].element[k];
			}

			double f = m_diagonal->col[i - 1].element[i - 1];
			double g = sqrt(h);

			if (f > 0.0)
			{
				g = -g;
			}

			m_offdiagonal->col[i].element[i] = scale * g;
			h = h - f * g;
			m_diagonal->col[i - 1].element[i - 1] = f - g;

			for (int j = 0; j < i; ++j)
			{
				m_offdiagonal->col[j].element[j] = 0.0;
			}

			// Apply similarity transformation to remaining columns.

			for (int j = 0; j < i; ++j)
			{
				f = m_diagonal->col[j].element[j];
				m_matrix->col[j].element[i] = f;
				g = m_offdiagonal->col[j].element[j] + m_matrix->col[j].element[j] * f;

				for (int k = j + 1; k <= i - 1; ++k)
				{
					g += m_matrix->col[k].element[j] * m_diagonal->col[k].element[k];
					m_offdiagonal->col[k].element[k] += m_matrix->col[k].element[j] * f;
				}

				m_offdiagonal->col[j].element[j] = g;
			}

			f = 0.0;

			for (int j = 0; j < i; ++j)
			{
				m_offdiagonal->col[j].element[j] /= h;
				f += m_offdiagonal->col[j].element[j] * m_diagonal->col[j].element[j];
			}

			double hh = f / (h + h);

			for (int j = 0; j < i; ++j)
			{
				m_offdiagonal->col[j].element[j] -= hh * m_diagonal->col[j].element[j];
			}

			for (int j = 0; j < i; ++j)
			{
				f = m_diagonal->col[j].element[j];
				g = m_offdiagonal->col[j].element[j];

				for (int k = j; k <= i - 1; ++k)
				{
					m_matrix->col[k].element[j] -= f * m_offdiagonal->col[k].element[k]
							+ g * m_diagonal->col[k].element[k];
				}

				m_diagonal->col[j].element[j] = m_matrix->col[i - 1].element[j];
				m_matrix->col[i].element[j] = 0;
			}
		}

		m_diagonal->col[i].element[i] = h;
	}

	// Accumulate transformations.

	for (int i = 0; i < 4; ++i)
	{
		m_matrix->col[3].element[i] = m_matrix->col[i].element[i];
		m_matrix->col[i].element[i] = 1.0;

		double h = m_diagonal->col[i + 1].element[i + 1];

		if (isnormal(h))
		{
			for (int k = 1; k <= i; ++k)
			{
				m_diagonal->col[k].element[k] = m_matrix->col[k].element[i + 1] / h;
			}

			for (int j = 1; j <= i; ++j)
			{
				double g = 0.0;

				for (int k = 1; k <= i; ++k)
				{
					g += m_matrix->col[k].element[i + 1] * m_matrix->col[k].element[j];
				}

				for (int k = 1; k <= i; ++k)
				{
					m_matrix->col[k].element[j] -= g * m_diagonal->col[k].element[k];
				}
			}
		}

		for (int k = 0; k <= i; ++k)
		{
			m_matrix->col[k].element[i + 1] = 0.0;
		}
	}

	for (int j = 0; j < 4; ++j)
	{
		m_diagonal->col[j].element[j] = m_matrix->col[3].element[j];
		m_matrix->col[3].element[j] = 0;
	}

	m_matrix->col[3].element[3] = 1.0;
	m_offdiagonal->col[1].element[1] = 0.0;
}

/**
 * Finds Eigenvalues and Eigenvectos of a symmetric, tri-diagonal
 * matrix by the QL method.
 *
 * @note
 * This is a translation of tql2.f from netlib which contained the
 * following notice:
 * this subroutine is a translation of the algol procedure tql2,
 * num. math. 11, 293-306(1968) by bowdler, martin, reinsch, and
 * wilkinson.
 * handbook for auto. comp., vol.ii-linear algebra, 227-240(1971).
 *
 * @param m_eigenvectors Contains transformation matrix from ebex_mat_tridiag_reduce, will be
 * 						 overwritten with the eigenvectors
 * @param m_eigenvalues Contains diagonal elements of the input matrix, will be overwritten with the eigenvalues
 * @param m_subdiag Contains the sub-diagonal elements of the input matrix
 */
void ebex_mat_tridiag_ql_4x4(ebex_mat4x4_t *m_eigenvectors, ebex_mat4x4_t *m_eigenvalues, ebex_mat4x4_t *m_subdiag)
{

	for (int i = 1; i < 4; i++)
	{
		m_subdiag->col[i - 1].element[i - 1] = m_subdiag->col[i].element[i];
	}

	m_subdiag->col[3].element[3] = 0.0;

	double f = 0.0;
	double tst1 = 0.0;
	double eps = pow(2.0, -52.0);

	for (int l = 0; l < 4; ++l)
	{
		// Find small subdiagonal element
		tst1 = MAX(tst1, fabs(m_eigenvalues->col[l].element[l]) + fabs(m_subdiag->col[l].element[l]));
		int m = l;

		while (m < 4)
		{
			if (fabs(m_subdiag->col[m].element[m]) <= eps * tst1)
			{
				break;
			}

			++m;
		}

		// If m == l, d[l] is an eigenvalue,
		// otherwise, iterate.
		if (m > l)
		{
			do
			{
				// Compute implicit shift

				double g = m_eigenvalues->col[l].element[l];
				double p = (m_eigenvalues->col[l + 1].element[l + 1] - g) / (2.0 * m_subdiag->col[l].element[l]);
				double r = hypot(p, 1.0);

				if (p < 0)
				{
					r = -r;
				}

				m_eigenvalues->col[l].element[l] = m_subdiag->col[l].element[l] / (p + r);
				m_eigenvalues->col[l + 1].element[l + 1] = m_subdiag->col[l].element[l] * (p + r);

				double dl1 = m_eigenvalues->col[l + 1].element[l + 1];
				double h = g - m_eigenvalues->col[l].element[l];

				for (int i = l + 2; i < 4; ++i)
				{
					m_eigenvalues->col[i].element[i] -= h;
				}

				f = f + h;

				// Implicit QL transformation.

				p = m_eigenvalues->col[m].element[m];
				double c = 1.0;
				double c2 = c;
				double c3 = c;
				double el1 = m_subdiag->col[l + 1].element[l + 1];
				double s = 0.0;
				double s2 = 0.0;

				for (int i = m - 1; i >= l; i--)
				{
					c3 = c2;
					c2 = c;
					s2 = s;
					g = c * m_subdiag->col[i].element[i];
					h = c * p;
					r = hypot(p, m_subdiag->col[i].element[i]);
					m_subdiag->col[i + 1].element[i + 1] = s * r;
					s = m_subdiag->col[i].element[i] / r;
					c = p / r;
					p = c * m_eigenvalues->col[i].element[i] - s * g;
					m_eigenvalues->col[i + 1].element[i + 1] = h + s * (c * g + s * m_eigenvalues->col[i].element[i]);

					// Accumulate transformation.
					for (int k = 0; k < 4; ++k)
					{
						h = m_eigenvectors->col[k].element[i + 1];
						m_eigenvectors->col[k].element[i + 1] = s * m_eigenvectors->col[k].element[i] + c * h;
						m_eigenvectors->col[k].element[i] = c * m_eigenvectors->col[k].element[i] - s * h;
					}
				}

				p = -s * s2 * c3 * el1 * m_subdiag->col[l].element[l] / dl1;
				m_subdiag->col[l].element[l] = s * p;
				m_eigenvalues->col[l].element[l] = c * p;

				// Check for convergence.

			} while (fabs(m_subdiag->col[l].element[l]) > eps * tst1);
		}

		m_eigenvalues->col[l].element[l] = m_eigenvalues->col[l].element[l] + f;
		m_subdiag->col[l].element[l] = 0.0;
	}

	// Sort eigenvalues and corresponding vectors.
	for (int i = 0; i < 4; ++i)
	{
		int k = i;

		double p = m_eigenvalues->col[i].element[i];

		for (int j = i + 1; j < 4; ++j)
		{
			if (m_eigenvalues->col[j].element[j] < p)
			{
				k = j;
				p = m_eigenvalues->col[j].element[j];
			}
		}

		if (k != i)
		{
			m_eigenvalues->col[k].element[k] = m_eigenvalues->col[i].element[i];
			m_eigenvalues->col[i].element[i] = p;

			for (int j = 0; j < 4; ++j)
			{
				p = m_eigenvectors->col[j].element[i];
				m_eigenvectors->col[j].element[i] = m_eigenvectors->col[j].element[k];
				m_eigenvectors->col[j].element[k] = p;
			}
		}
	}
}

int ebex_3mat_sqrt( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat)
{
	  const int MAX_ITER = 100;
	  int iter;
	  double sse, psse;
	  int i;
	  ebex_mat3x3_t x, xinv, axinv, xtemp, error;

	  ebex_3mat_make_identity(&x);

	  psse = 1.0e+30;
	  for (iter = 0;  iter < MAX_ITER;  iter++)
	    {
		  ebex_3mat_inverse(&xinv, &x);
		  ebex_3mat_mul(&axinv, m_mat, &xinv);
		  ebex_3mat_add(&xtemp, &x, &axinv);
		  ebex_3mat_scalar_mul(&x, &xtemp, 0.5);

		  ebex_3mat_mul(&xtemp, &x, &x);
		  ebex_3mat_sub(&error, m_mat, &xtemp);

	      sse = 0.0;
	      for (i = 0;  i < 3;  i++)
	      {
	          sse += error.col[0].element[i] * error.col[0].element[i];
	          sse += error.col[1].element[i] * error.col[1].element[i];
	          sse += error.col[2].element[i] * error.col[2].element[i];
	      }

	      if (sse >= psse) break;

	      psse = sse;
	    }

	  if (iter == MAX_ITER)  return -1;

	  ebex_3mat_copy(m_result, &x);
	  return 0;
}

/* Construct 3x3 matrix from Euler angles (in radians). */
void ebex_3mat_from_euler(ebex_mat3x3_t *m_result, const ebex_3vec_t *m_src, e_rotation_order m_order)
{
	const rotation_order_t *R = GET_ROT_ORDER(m_order);
	double ci, cj, ck, si, sj, sk;
	double cc, cs, sc, ss;
	const double *e = m_src->element;

	if (R->parity)
	{

		sincos(-e[R->axis[0]], &si, &ci);
		sincos(-e[R->axis[1]], &sj, &cj);
		sincos(-e[R->axis[2]], &sk, &ck);
	}
	else
	{
		vdSinCos(1, &e[R->axis[0]], &si, &ci);
		vdSinCos(1, &e[R->axis[1]], &sj, &cj);
		vdSinCos(1, &e[R->axis[2]], &sk, &ck);
	}

	cc = ci * ck;
	cs = ci * sk;
	sc = si * ck;
	ss = si * sk;

	ebex_3vec_make_from_elems(&m_result->col[0], cj * ck, cj * sk, -sj);
	ebex_3vec_make_from_elems(&m_result->col[1], sj * sc - cs, sj * ss + cc, cj * si);
	ebex_3vec_make_from_elems(&m_result->col[2], sj * cc + ss, sj * cs - sc, cj * ci);

}


